#ifndef _NAV_BASE_HPP
#define _NAV_BASE_HPP

#include <iostream>
#include <memory>
#include <eigen3/Eigen/Dense>

class ImuData{
public:
    double timestamp;
    Eigen::Vector3d gyro_rps;
    Eigen::Vector3d accel_mpss;
};

class OdomData{
public:
    double timestamp;
    Eigen::Vector3d vel;
    bool valid;
};

class ImuModel{
public:
    double ARW;                             // 角度随机游走
    double VRW;                             // 速度随机游走
    double gyro_bias_std;                   // 陀螺仪零偏标准差
    double gyro_bias_corr_time;             // 陀螺仪零偏相关时间
    double accel_bias_std;                  // 加速度计零偏标准差
    double accel_bias_corr_time;            // 加速度计零偏相关时间
    double odom_scale_std;                   // 里程计比例因子随机游走
    double odom_std;                        // 里程计观测噪声标准差
public:
    ImuModel(){}
};

class State{
public:
    double timestamp;
    Eigen::Vector3d     pos;
    Eigen::Vector3d     vel;    // 北东地       :m/s
    Eigen::Vector3d     att;    // rol pit yaw :rad
    Eigen::Matrix3d     c_bn;   // 旋转矩阵
    Eigen::Quaterniond  q_bn;   // 旋转四元数
    Eigen::Vector3d     gyro_bias;          // 陀螺仪零偏
    Eigen::Vector3d     accel_bias;         // 加速度计零偏
    double              odom_scale_factor;
    Eigen::Vector3d     imu_mount_angle;    // imu安装角 imu相对于车体的转动角
    Eigen::Matrix3d     c_bv;               // 安装角对应的旋转矩阵 body -> vehicle
    Eigen::Vector3d     odom_lever_arm;     // 里程计杆臂 里程计中心相对IMU的坐标
};
using StatePtr=std::shared_ptr<State>;


class NavBase{
protected:
    double r2d;
    double d2r;

    NavBase(){
        r2d = 180.0 / 3.1415926535;
        d2r = 3.1415926535 / 180.0;
    }
    
    void dcm2euler(const Eigen::Matrix3d& dcm, Eigen::Vector3d& euler){
        // 范围
        // rol: -pi~pi
        // pit: -pi/2~pi/2
        // yaw: -pi~pi
        double rol = atan2( dcm(2, 1), dcm(2, 2));
        double pit = atan2(-dcm(2, 0), sqrt(dcm(2, 1) * dcm(2, 1) + dcm(2, 2) * dcm(2, 2)));
        double yaw = atan2( dcm(1, 0), dcm(0, 0));

        euler << yaw, pit, rol;
    }

    /**
     * @brief 若n->b: (φ θ ψ)，则返回C_bn
     * 
     * @param euler 
     * @param dcm 
     */
    void euler2dcm(const Eigen::Vector3d& euler, Eigen::Matrix3d& dcm){
        double rol = euler[2];
        double pit = euler[1];
        double yaw = euler[0];
        double c11 =  cos(pit) * cos(yaw);
        double c12 = -cos(rol) * sin(yaw) + sin(rol) * sin(pit) * cos(yaw);
        double c13 =  sin(rol) * sin(yaw) + cos(rol) * sin(pit) * cos(yaw);
        double c21 =  cos(pit) * sin(yaw);
        double c22 =  cos(rol) * cos(yaw) + sin(rol) * sin(pit) * sin(yaw);
        double c23 = -sin(rol) * cos(yaw) + cos(rol) * sin(pit) * sin(yaw);
        double c31 = -sin(pit);
        double c32 =  sin(rol) * cos(pit);
        double c33 =  cos(rol) * cos(pit);

        dcm <<  c11, c12, c13,
                c21, c22, c23,
                c31, c32, c33;

    }

    // 获取反三角矩阵
    Eigen::Matrix3d getMX(const Eigen::Vector3d& vec){
        Eigen::Matrix3d mat;
        mat <<        0, -vec[2],  vec[1],
                 vec[2],       0, -vec[0],
                -vec[1],  vec[0],       0;
        return mat;
    }

    void quat2dcm(const Eigen::Quaterniond& quat, Eigen::Matrix3d& dcm) {
        double q0 = quat.w();
        double q1 = quat.x();
        double q2 = quat.y();
        double q3 = quat.z();
        dcm <<  q0*q0 + q1*q1 - q2*q2 - q3*q3   , 2*(q1*q2-q0*q3)               , 2*(q1*q3+q0*q2)               ,
                2*(q1*q2+q0*q3)                 , q0*q0 - q1*q1 + q2*q2 - q3*q3 , 2*(q2*q3-q0*q1)               ,
                2*(q1*q3-q0*q2)                 , 2*(q2*q3+q0*q1)               , q0*q0 - q1*q1 - q2*q2 + q3*q3 ;
    }

    void quat2euler(const Eigen::Quaterniond& quat, Eigen::Vector3d& euler) {
        Eigen::Matrix3d dcm;
        quat2dcm(quat, dcm);
        dcm2euler(dcm, euler);
    }   


    void euler2quat(const Eigen::Vector3d& euler, Eigen::Quaterniond& quat){
        auto yaw    = euler[0];
        auto roll   = euler[1];
        auto pitch  = euler[2];
        quat.x()=sin(pitch/2)*sin(yaw/2)*cos(roll/2)+cos(pitch/2)*cos(yaw/2)*sin(roll/2);
        quat.y()=sin(pitch/2)*cos(yaw/2)*cos(roll/2)+cos(pitch/2)*sin(yaw/2)*sin(roll/2);
        quat.z()=cos(pitch/2)*sin(yaw/2)*cos(roll/2)-sin(pitch/2)*cos(yaw/2)*sin(roll/2);
        quat.w()=cos(pitch/2)*cos(yaw/2)*cos(roll/2)-sin(pitch/2)*sin(yaw/2)*sin(roll/2);
    }

};

#endif